//Application
#include "manipulator.h"
#include "robot_cmd.h"
#include "robot_def.h"
//modules
#include "matrix.h"
#include "message_center.h"
#include "arm_solution.h"
#include "step_motor.h"
//core

static Publisher_t *manipulator_pub;
static Manipulator_Ctrl_Cmd_s Manipulator_cmd_recv; // 来自cmd发布的控制信息
static Subscriber_t *manipulator_sub;
static Manipulator_Upload_Data_s Manipulator_feedback_data; // 来自电机的反馈信息信息

static Subscriber_t *manipulator_sub1;
static Manipulator_Upload_Data_s Manipulator_feedback_data1; // 来自电机的反馈信息信息

static StepInstance* step1,*step2,*step3,*step4,*step5,*step6;   //六轴电机实例

/* 机械臂初始化函数 */
void ManipulatorInit()
{   
    Step_Init_Config_s step_conf4 = {
        .htim = &htim8,
        .Channel = TIM_CHANNEL_1,
        .DIR.GPIOx = GPIOC,
        .DIR.GPIO_Pin = GPIO_PIN_1,
        .ENA.GPIOx = GPIOC,
        .ENA.GPIO_Pin = GPIO_PIN_5,
        .Init_frequency = 100000,
        .Init_perangle = 0.225,
        .mode = 2,
        .sareturn.pulse_count = 0,
        .sareturn.pulse_ccr = 0,
        .dir_flag = 1,
    };
    step4 = StepInit(&step_conf4);

    Step_Init_Config_s step_conf5 = {
        .htim = &htim8,
        .Channel = TIM_CHANNEL_2,
        .DIR.GPIOx = GPIOA,
        .DIR.GPIO_Pin = GPIO_PIN_4,
        .ENA.GPIOx = GPIOA,
        .ENA.GPIO_Pin = GPIO_PIN_5,
        .Init_frequency = 100000,
        .Init_perangle = 0.225,
        .mode = 2,
        .sareturn.pulse_count = 0,
        .sareturn.pulse_ccr = 0,
        .dir_flag = 1,
    };
    step5 = StepInit(&step_conf5);

    Step_Init_Config_s step_conf6 = {
        .htim = &htim8,
        .Channel = TIM_CHANNEL_3,
        .DIR.GPIOx = GPIOI,
        .DIR.GPIO_Pin = GPIO_PIN_9,
        .ENA.GPIOx = GPIOF,
        .ENA.GPIO_Pin = GPIO_PIN_10,
        .Init_frequency = 100000,
        .Init_perangle = 0.225,
        .mode = 2,
        .sareturn.pulse_count = 0,
        .sareturn.pulse_ccr = 0,
        .dir_flag = 1,
    };
    step6 = StepInit(&step_conf6);

    Step_Init_Config_s step_conf1 = {
        .htim = &htim4,
        .Channel = TIM_CHANNEL_1,
        .DIR.GPIOx = GPIOF,
        .DIR.GPIO_Pin = GPIO_PIN_0,
        .ENA.GPIOx = GPIOF,
        .ENA.GPIO_Pin = GPIO_PIN_1,
        .Init_frequency = 100000,
        .Init_perangle = 0.225,
        .mode = 2,
        .sareturn.pulse_count = 0,
        .sareturn.pulse_ccr = 0,
        .dir_flag = 1,
    };
    step1 = StepInit(&step_conf1);

    Step_Init_Config_s step_conf2 = {
        .htim = &htim4,
        .Channel = TIM_CHANNEL_2,
        .DIR.GPIOx = GPIOE,
        .DIR.GPIO_Pin = GPIO_PIN_4,
        .ENA.GPIOx = GPIOE,
        .ENA.GPIO_Pin = GPIO_PIN_5,
        .Init_frequency = 100000,
        .Init_perangle = 0.225,
        .mode = 2,
        .sareturn.pulse_count = 0,
        .sareturn.pulse_ccr = 0,
        .dir_flag = 1,
    };
    step2 = StepInit(&step_conf2);

    Step_Init_Config_s step_conf3 = {
        .htim = &htim4,
        .Channel = TIM_CHANNEL_3,
        .DIR.GPIOx = GPIOE,
        .DIR.GPIO_Pin = GPIO_PIN_12,
        .ENA.GPIOx = GPIOE,
        .ENA.GPIO_Pin = GPIO_PIN_6,
        .Init_frequency = 100000,
        .Init_perangle = 0.225,
        .mode = 2,
        .sareturn.pulse_count = 0,
        .sareturn.pulse_ccr = 0,
        .dir_flag = 1,
    };
    step3 = StepInit(&step_conf3);



    
    /* 注册机械臂话题 */
    manipulator_pub = PubRegister("manipulator_feed", sizeof(Manipulator_Upload_Data_s));
    manipulator_sub = SubRegister("manipulator_cmd", sizeof(Manipulator_Ctrl_Cmd_s));
    manipulator_sub1= SubRegister("manipulator_feed", sizeof(Manipulator_Upload_Data_s));
}

/* 机械臂任务 */
float count = 69;
void ManipulatorTask()
{
    // 从cmd获取电机目标角度
    SubGetMessage(manipulator_sub, &Manipulator_cmd_recv);
    // 获取电机此时角度
    SubGetMessage(manipulator_sub1, &Manipulator_feedback_data1);
    // 控制电机转动
    if(1)
    {
        Set_abs_SA(step4,count+=2,360);
        Set_abs_SA(step5,count+=2,360);
        Set_abs_SA(step6,count+=2,360);
        Set_abs_SA(step1,count+=2,360);
        Set_abs_SA(step2,count+=2,360);
        Set_abs_SA(step3,count+=2,360);
   
    }
    else if(Manipulator_cmd_recv.mode == STAST_MODE)
    {
        //StepMotor_Start(step1);
    }
    else if(Manipulator_cmd_recv.mode == TASK_MODE)
    {
        //转动到固定角

        //记录此时角度
        step1->angle = Manipulator_cmd_recv.angle_de[0];
        step2->angle = Manipulator_cmd_recv.angle_de[1];
        step3->angle = Manipulator_cmd_recv.angle_de[2];
        step4->angle = Manipulator_cmd_recv.angle_de[3];
        step5->angle = Manipulator_cmd_recv.angle_de[4];
        step6->angle = Manipulator_cmd_recv.angle_de[5];
        //发布电机角度
        Manipulator_feedback_data.angle_now[0] = step1->angle;
        Manipulator_feedback_data.angle_now[1] = step2->angle;
        Manipulator_feedback_data.angle_now[2] = step3->angle;
        Manipulator_feedback_data.angle_now[3] = step4->angle;
        Manipulator_feedback_data.angle_now[4] = step5->angle;
        Manipulator_feedback_data.angle_now[5] = step6->angle;
    }
    else if(Manipulator_cmd_recv.mode == STOP_MODE)
    {
        // StepMotor_Stop(step1);
        // StepMotor_Stop(step2);
        // StepMotor_Stop(step3);
        // StepMotor_Stop(step4);
        // StepMotor_Stop(step5);
        // StepMotor_Stop(step6);
    }
    // 更新电机角度数据
    PubPushMessage(manipulator_pub, (void *)&Manipulator_feedback_data);

}

